#!/usr/bin/env python

import rospy
from flydatas_msgs.msg import Flydatas  # 假设消息类型是 flydatas_msgs/Flydatas
from sensor_msgs.msg import NavSatFix

# 回调函数处理接收到的自定义消息
def flydatas_callback(msg):
    # 处理 GPS 数据
    gps1_lat = msg.gps[0].latitude
    gps1_lon = msg.gps[0].longitude
    gps1_alt = msg.gps[0].altitude
    
    gps2_lat = msg.gps[1].latitude
    gps2_lon = msg.gps[1].longitude
    gps2_alt = msg.gps[1].altitude
    
    gps3_lat = msg.gps[2].latitude
    gps3_lon = msg.gps[2].longitude
    gps3_alt = msg.gps[2].altitude
    
    gps4_lat = msg.gps[3].latitude
    gps4_lon = msg.gps[3].longitude
    gps4_alt = msg.gps[3].altitude

    # 打印 GPS 数据
    rospy.loginfo("GPS 1 - Latitude: %f, Longitude: %f, Altitude: %f", gps1_lat, gps1_lon, gps1_alt)
    rospy.loginfo("GPS 2 - Latitude: %f, Longitude: %f, Altitude: %f", gps2_lat, gps2_lon, gps2_alt)
    rospy.loginfo("GPS 3 - Latitude: %f, Longitude: %f, Altitude: %f", gps3_lat, gps3_lon, gps3_alt)
    rospy.loginfo("GPS 4 - Latitude: %f, Longitude: %f, Altitude: %f", gps4_lat, gps4_lon, gps4_alt)
    dd = msg.doit
    rospy.loginfo("doit:%f",dd)
    # 检查飞行状态
    armed = msg.flight_state.armed
    mode = msg.flight_state.mode

    # # 打印飞行状态信息
    # rospy.loginfo("Flight State - Armed: %s,  Mode: %s", armed, mode)

def listener():
    # 初始化 ROS 节点
    rospy.init_node('flydatas_subscriber_node', anonymous=True)
    
    # 订阅自定义消息
    rospy.Subscriber("/sub_flydatas", Flydatas, flydatas_callback)
    
    # 保持节点运行，等待消息
    rospy.spin()

if __name__ == '__main__':
    try:
        listener()
    except rospy.ROSInterruptException:
        pass
